const int BIG_WHEEL_1_INCH = 91;  //constants for converting distance to encoders
const int SMALL_WHEEL_1_INCH = 121;
const float LARGE_BASE_1_DEGREE = 12.55;
const int SMALL_BASE_1_DEGREE = 4;//Currently not used, hasn't been updated
int isSmallFrame = false;


void halt() // stops all drive motors
{
	motor[Left] = 0;
	motor[Right] = 0;
	motor[Front] = 0;
	motor[Rear] = 0;
}

int getEncoderValueForAngle(int angleDegrees) // converts an angle to an encoder value
{
	if (isSmallFrame)
	{
		return angleDegrees * SMALL_BASE_1_DEGREE;
	}
	else
	{
		return angleDegrees * LARGE_BASE_1_DEGREE;
	}
}

int getEncoderValueForDistance(int distanceInches)//Converts a distance in inches to an encoder value.
{
	if (isSmallFrame)
	{
		return distanceInches * SMALL_WHEEL_1_INCH;
	}
	else
	{
		return distanceInches * BIG_WHEEL_1_INCH;
	}
}

void movement(int scaledX ,int scaledY,int scaledTheta,int joystick2_y)//Moves the robot based on joystick values.
{
			motor[Front] = (-scaledY - scaledX - scaledTheta);
			motor[Right] = (scaledY - scaledX - scaledTheta);
			motor[Left] = (-scaledY + scaledX - scaledTheta);
			motor[Rear] = (scaledY + scaledX - scaledTheta);
}

void translateForward(int motorPower, int moveTime)//Moves the robot forwards at a certain power for a certain time.
{
	motor[Left] = motorPower * -1;
	motor[Right] = motorPower;
	wait1Msec(moveTime);
	halt();
}

void translateForwardDistanceBased(int motorPower, int distanceValue)//Moves the robot forward at a set power for a distance.
{
	nMotorEncoder[Left] = 0;
	motor[Left] = motorPower * -1;
	motor[Right] = motorPower;
	while (abs(nMotorEncoder[Left]) < getEncoderValueForDistance(distanceValue))
	{

	}
	halt();
}

void translateBackwards(int motorPower, int moveTime)//Moves the robot backwards at a certain power for a certain time.
{
	motor[Left] = motorPower;
	motor[Right] = motorPower * -1;
	wait1Msec(moveTime);
	halt();
}

void translateBackwardsDistanceBased(int motorPower, int distanceValue)//Translates the robot backwards using encoders.
{
	nMotorEncoder[Left] = 0;
	motor[Left] = motorPower;
	motor[Right] = motorPower * -1;
	while (abs(nMotorEncoder[Left]) < getEncoderValueForDistance(distanceValue))
	{

	}
	halt();
}

void translateLeft(int motorPower, int moveTime)//Translates the robot left using time.
{
	motor[Front] = motorPower;
	motor[Rear] = motorPower * -1;
	wait1Msec(moveTime);
	halt();
}

void translateLeftDistanceBased(int motorPower, int distanceValue)//Translates the robot left using encoders.
{
	nMotorEncoder[Front] = 0;
	motor[Front] = motorPower;
	motor[Rear] = motorPower * -1;
	while (abs(nMotorEncoder[Front]) < getEncoderValueForDistance(distanceValue))
	{

	}
	halt();
}

void translateRight(int motorPower, int moveTime)//Translates the robot right using time.
{
	motor[Front] = motorPower * -1;
	motor[Rear] = motorPower;
	wait1Msec(moveTime);
	halt();
}

void translateRightDistanceBased(int motorPower, int distanceValue)//Translates the robot right using encoders.
{
 	nMotorEncoder[Front] = 0;
	motor[Front] = motorPower * -1;
	motor[Rear] = motorPower;
	while (abs(nMotorEncoder[Front]) < getEncoderValueForDistance(distanceValue))
	{

	}
	halt();
}

void rotateClockwise(int motorPower, int moveTime)//Rotates the robot clockwise using time.
{
	motor[Left] = motorPower;
	motor[Right] = motorPower;
	motor[Front] = motorPower;
	motor[Rear] = motorPower;
	wait1Msec(moveTime);
	halt();
}

void rotateClockwiseAngleBased(int motorPower, int angle)//Rotates the robot clockwise using encoders.
{
	nMotorEncoder[Left] = 0;
	motor[Left] = motorPower;
	motor[Right] = motorPower;
	motor[Front] = motorPower;
	motor[Rear] = motorPower;
	while (abs(nMotorEncoder[Left]) < getEncoderValueForAngle(angle))
	{

	}
	halt();
}

void rotateCounterClockwise(int motorPower, int moveTime)//Rotates the robot counter-clockwise using time.
{
	motor[Left] = motorPower * -1;
	motor[Right] = motorPower * -1;
	motor[Front] = motorPower * -1;
	motor[Rear] = motorPower * -1;
	wait1Msec(moveTime);
	halt();
}

void rotateCounterClockwiseAngleBased(int motorPower, int angle)//Rotates the robot counter-clockwise using encoders.
{
	nMotorEncoder[Left] = 0;
	motor[Left] = motorPower * -1;
	motor[Right] = motorPower * -1;
	motor[Front] = motorPower * -1;
	motor[Rear] = motorPower * -1;
	while (abs(nMotorEncoder[Left]) < getEncoderValueForAngle(angle))
	{

	}
	halt();
}
void moveOffRamp(int motorPower, int moveTime)//Moves robot off ramp for autonomous routine.
{
	motor[Left] = motorPower * -1;
	motor[Right] = motorPower * 1;
	motor[Front] = motorPower * -1;
	motor[Rear] = motorPower * 1;
	wait1Msec(moveTime);
	halt();
}
